﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Skill.Ode;
using Skill.Ode.Joints;
using Microsoft.Xna.Framework;
using Skill.Ode.Geometries;
using Microsoft.Xna.Framework.Graphics;
using Skill.Ode.Extension;
using Microsoft.Xna.Framework.Input;
using Samples.Common;

namespace Samples.Demos
{
    public class Hinge : OdeDemo
    {
        #region Variables
        // some constants
        const float SIDE = 0.5f;	// side length of a box
        const float MASS = 1.0f;	// mass of a box

        // dynamics and collision objects
        private Ode ode;
        private DBody[] bodies;
        private DHinge hinge;

        private DisplayGeometry[] boxes;

        bool occasional_error;
        float a = 0;
        #endregion

        #region Constructor
        public Hinge(DemoFramework framework)
            : base(framework)
        {
            mapper.MapKeyPress(Keys.R, delegate { occasional_error = true; });
        }
        #endregion

        #region OdeDemo members

        #region StepDemo
        public override void StepDemo(Microsoft.Xna.Framework.GameTime gameTime)
        {
            float kd = -0.3f;	// angular damping constant  
            // add an oscillating torque to body 0, and also damp its rotational motion            
            DVector3 w = bodies[0].AngularVelocity;
            bodies[0].AddTorque(kd * w.X, kd * w.Y + 0.1f * (float)Math.Cos(a),
                kd * w.Z + 0.1f * (float)Math.Sin(a));
            ode.World.Step(0.05f);
            a += 0.01f;

            mapper.Update(ref Framework.Keyboard);

            //occasionally re-orient one of the bodies to create a deliberate error.
            if (occasional_error)
            {
                int count = 0;
                if ((count % 20) == 0)
                {
                    //randomly adjust orientation of body[0]            
                    DMatrix3 R2 = DMatrix3.FromAxisAndAngle(DRandom.GetRandReal() - 0.5f, DRandom.GetRandReal() - 0.5f,
                                DRandom.GetRandReal() - 0.5f, DRandom.GetRandReal() - 0.5f);
                    DMatrix3 R3 = new DMatrix3();
                    Skill.Ode.DMatrix3.Multiply0(R3, bodies[0].Rotation, R2);
                    bodies[0].Rotation = R3;

                    // randomly adjust position of body[0]
                    DVector3 pos = new DVector3(bodies[0].Position);
                    bodies[0].Position.Set(pos.X + 0.2f * (DRandom.GetRandReal() - 0.5f),
                              pos.Y + 0.2f * (DRandom.GetRandReal() - 0.5f),
                              pos.Z + 0.2f * (DRandom.GetRandReal() - 0.5f));
                }
                count++;
                occasional_error = false;
            }                        
            boxes[0].Transform = bodies[0].WorldMatrix.ToXna();
            boxes[1].Transform = bodies[1].WorldMatrix.ToXna();            
        }
        #endregion

        #region Initialize
        public override void Initialize()
        {
            a = 0;
            occasional_error = false;

            Vector3 extent = new Vector3(SIDE, SIDE, SIDE);

            bodies = new DBody[2];
            boxes = new DisplayGeometry[2];
            boxes[0] = new DisplayBox(Framework.GraphicsDevice, Color.LightGray, extent);
            boxes[1] = new DisplayBox(Framework.GraphicsDevice, Color.Gold, extent);
            Framework.Add(boxes[0]);
            Framework.Add(boxes[1]);

            ode = new Ode();            

            DMass m = new DMass();
            m.SetBox(1, SIDE, SIDE, SIDE);
            m.Adjust(MASS);

            DQuaternion q = DQuaternion.FromAxisAndAngle(1, 1, 0, MathHelper.PiOver4);

            bodies[0] = new DBody(ode.World);
            bodies[0].Mass = m;
            DMass mass = bodies[0].Mass;
            bodies[0].Position = new DVector3(0.5f * SIDE, 0.5f * SIDE, 1);
            bodies[0].Quaternion = q;

            bodies[1] = new DBody(ode.World);
            bodies[1].Mass = m;
            bodies[1].Position = new DVector3(-0.5f * SIDE, -0.5f * SIDE, 1);
            bodies[1].Quaternion = q;

            hinge = new DHinge(ode.World, null);
            hinge.Attach(bodies[0], bodies[1]);
            hinge.Anchor1 = new DVector3(0, 0, 1);
            hinge.Axis = new DVector3(1, -1, 1.41421356f);
        }
        #endregion

        #region Quit
        public override void Quit()
        {
            hinge.Dispose();
            bodies[0].Dispose();
            bodies[1].Dispose();
            ode.Dispose();
        }
        #endregion
        #endregion
    }
}
